#include "troypose.h"

#include <QQuaternion>
#include <qmath.h>


const float TroyPose::jointRadius = 0.5f;
const float TroyPose::upperArmLength = 3.1f;
const float TroyPose::lowerArmLength = 5.1f;

TroyPose::TroyPose() :
    needsRaw(false)
{
}

TroyPose::TroyPose(QSharedPointer<UserSkeleton> skeleton) {
    setUserSkeleton(skeleton);
}

TroyPose::TroyPose(QSharedPointer<UserSkeleton> skeleton, QMap<int, float> hints) {
    setUserSkeleton(skeleton, hints);
}

TroyPose::~TroyPose() {
}

void TroyPose::setJointAngle(int channel, float theta) {
    RobotPose::setJointAngle(channel, theta);
    clearCache(channel);
}

bool TroyPose::hasExpression() const {
    return !expressionPath.isNull();
}

QString TroyPose::expression() const {
    return expressionPath;
}

void TroyPose::setExpression(QString expr) {
    expressionPath = expr;
}

bool TroyPose::hasJointPosition(TroyPoint joint) const {
    return positions.contains(joint);
}

QVector3D TroyPose::jointPosition(TroyPoint joint) const {
    if (hasJointPosition(joint)) {
        return positions.find(joint).value();
    } else {
        // Here is where the position should be generated
        if (canMakePosition(joint)) {
            return makePosition(joint);
        } else {
            return QVector3D();
        }
    }
}

bool TroyPose::hasRawPose() const {
    return needsRaw;
}

QSharedPointer<TroyPose const> TroyPose::rawPose() const {
    // Check to see if the raw skeleton has been cached yet
    if (needsRaw && raw.isNull()) {
        QSharedPointer<TroyPose> newRaw(new TroyPose());
        newRaw->setUserSkeleton(user, false);
        raw.swap(newRaw);
    }
    return raw;
}

bool TroyPose::hasUserSkeleton() const {
    return !user.isNull();
}

QSharedPointer<UserSkeleton const> TroyPose::userSkeleton() const {
    return user;
}

//void TroyPose::setJointPosition(TroyPoint joint, QVector3D position) {
//    positions.insert(joint, position);
//}

//void TroyPose::setRawSkeleton(QSharedPointer<TroyPose> pose) {
//    raw.swap(pose);
//}

void TroyPose::setUserSkeleton(QSharedPointer<UserSkeleton> skeleton, bool restrain) {
    setUserSkeleton(skeleton, QMap<int, float>(), restrain);
}

// Before you begin reading too much of this series of retargetting methods, let me just say that they have been
// cleaned up considerably to get this into this present state.
// If anyone else needs to maintain this and I'm long gone, my email is tyler.gill@byu.net, if you'd like me
// to walk you through the code.
void TroyPose::setUserSkeleton(QSharedPointer<UserSkeleton> skeleton, QMap<int, float> hints, bool restrain) {
    if (skeleton->hasUserPosition(UserSkeleton::Torso)
        && skeleton->hasUserPosition(UserSkeleton::Neck)
        && skeleton->hasUserPosition(UserSkeleton::LeftShoulder)
        && skeleton->hasUserPosition(UserSkeleton::RightShoulder)) {

        // This is what will be stored as the raw pose, so as we go along, any time we would 'snap' a joint
        // into the constraints of Troy, we will maintain two states so that both joint variants can be
        // remembered. This second 'pseudo state' is maintained in the set of variables whose names end in
        // 'Snapped', following the first, upper snapping.
        //QSharedPointer<TroyPose> newRaw(new TroyPose());
        //setRawSkeleton(newRaw);

        // The raw skeleton will be lazily loaded when it is requested, as all the required data is stored anyway.
        needsRaw = false;

        user = skeleton;

        // Get the vectors we need for the shoulder:
        // This is the backbone (in the downward direction) used as a reference for the shoulder
        QVector3D backbone(skeleton->userPosition(UserSkeleton::Torso) - skeleton->userPosition(UserSkeleton::Neck));
        backbone.normalize();
        QVector3D leftOut(skeleton->userPosition(UserSkeleton::LeftShoulder) - skeleton->userPosition(UserSkeleton::RightShoulder));
        leftOut.normalize();
        QVector3D rightOut(-leftOut);
        rightOut.normalize();
        QVector3D forward(QVector3D::normal(backbone, leftOut));

        // This is used to adjust Troys torso ratio so that the more the users hands are in front of their body, the more Troys hands are in front of his.
        //float torsoWidth = qAbs((skeleton->userPosition(UserSkeleton::LeftShoulder) - skeleton->userPosition(UserSkeleton::RightShoulder)).length());

        // Troy is restricted to -20 - 180 degrees for the rsf and lsf angles, so we snap to the closer one when the arm is bent back like that
        QVector3D snapCheck(qSin(190.0f*(M_PI/180.0f))*backbone + qCos(190.0f*(M_PI/180.0f))*forward);
        snapCheck.normalize();

        if (skeleton->hasUserPosition(UserSkeleton::LeftElbow)
            && skeleton->hasUserPosition(UserSkeleton::LeftHand)) {

            QVector3D leftShoulder;
            QVector3D leftUpperArm;
            QVector3D leftShoulderTwistVector;
            needsRaw |= buildUpperArmVectors(
                        skeleton,
                        UserSkeleton::LeftShoulder,
                        UserSkeleton::LeftElbow,
                        UserSkeleton::LeftHand,
                        backbone,
                        leftOut,
                        forward,
                        snapCheck,
                        leftShoulder,
                        leftUpperArm,
                        leftShoulderTwistVector,
                        restrain,
                        hints.contains(LeftShoulderForward) ? hints.find(LeftShoulderForward).value() : FLT_MAX);
            setJointAngle(LeftShoulderForward, finalizeJointAngle(leftShoulderTwistVector, forward, backbone));
            setJointAngle(LeftShoulderOut, finalizeJointAngle(leftUpperArm, leftOut, leftShoulderTwistVector));
            QVector3D leftLowerArm;
            QVector3D leftLowerSignVector;
            QVector3D leftLowerStart;
            QVector3D leftLowerTwistVector;
            buildLowerArmVectors(
                        skeleton,
                        UserSkeleton::LeftElbow,
                        UserSkeleton::LeftHand,
                        leftOut,
                        leftUpperArm,
                        leftShoulderTwistVector,
                        leftLowerArm,
                        leftLowerSignVector,
                        leftLowerStart,
                        leftLowerTwistVector);
            float lt = finalizeJointAngle(leftLowerTwistVector, leftLowerSignVector, leftLowerStart);
            // This handles snap-proofing so that the joint doesn't jump between positive and negative 180
            // The magic number 225 means that if the joint gets to be more than 45 degrees off (180 +/- 45) then it will
            // just go where it should be.
            if (hints.contains(LeftTwist) && restrain) {
                float oldLt = hints.find(LeftTwist).value();
                if (qAbs(oldLt-lt) > 225.0f) {
                    needsRaw = true;
                    if (oldLt > 0) {
                        //lt = oldLt - qAbs(oldLt-lt)/(float)15; //picked arbitrarily at random
                        lt = 180.0f;
                    } else {
                        //lt = oldLt + qAbs(oldLt-lt)/(float)15; //picked arbitrarily at random
                        lt = -180.0f;
                    }
                }
            }
            setJointAngle(LeftTwist, lt);
            setJointAngle(LeftElbow, -1 * angleDifference(leftLowerArm, leftUpperArm));
        }

        if (skeleton->hasUserPosition(UserSkeleton::RightElbow)
            && skeleton->hasUserPosition(UserSkeleton::RightHand)) {

            QVector3D rightShoulder;
            QVector3D rightUpperArm;
            QVector3D rightShoulderTwistVector;
            needsRaw |= buildUpperArmVectors(
                        skeleton,
                        UserSkeleton::RightShoulder,
                        UserSkeleton::RightElbow,
                        UserSkeleton::RightHand,
                        backbone,
                        rightOut,
                        forward,
                        snapCheck,
                        rightShoulder,
                        rightUpperArm,
                        rightShoulderTwistVector,
                        restrain,
                        hints.contains(RightShoulderForward) ? hints.find(RightShoulderForward).value() : FLT_MAX);
            setJointAngle(RightShoulderForward, finalizeJointAngle(rightShoulderTwistVector, forward, backbone));
            setJointAngle(RightShoulderOut, finalizeJointAngle(rightUpperArm, rightOut, rightShoulderTwistVector));
            QVector3D rightLowerArm;
            QVector3D rightLowerSignVector;
            QVector3D rightLowerStart;
            QVector3D rightLowerTwistVector;
            buildLowerArmVectors(
                        skeleton,
                        UserSkeleton::RightElbow,
                        UserSkeleton::RightHand,
                        rightOut,
                        rightUpperArm,
                        rightShoulderTwistVector,
                        rightLowerArm,
                        rightLowerSignVector,
                        rightLowerStart,
                        rightLowerTwistVector);
            // This needs the inversion on the sign vector to factor for....something.
            // I refactored this a long time after my initial design of all this, and I didn't think this needed to be here,
            // but for some reason it only worked when it was here.
            // Feel free to figure it out, or just have faith and accept the magic.
            float rt = finalizeJointAngle(rightLowerTwistVector, -1 * rightLowerSignVector, rightLowerStart);
            // This handles snap-proofing so that the joint doesn't jump between positive and negative 180
            // The magic number 225 means that if the joint gets to be more than 45 degrees off (180 +/- 45) then it will
            // just go where it should be.
            if (hints.contains(RightTwist) && restrain) {
                float oldRt = hints.find(RightTwist).value();
                if (qAbs(oldRt-rt) > 225.0f) {
                    needsRaw = true;
                    if (oldRt > 0.0f) {
                        //lt = oldLt - qAbs(oldLt-lt)/(float)15; //picked arbitrarily at random
                        rt = 180.0f;
                    } else {
                        //lt = oldLt + qAbs(oldLt-lt)/(float)15; //picked arbitrarily at random
                        rt = -180.0f;
                    }
                }
            }
            setJointAngle(RightTwist, rt);
            setJointAngle(RightElbow, -1 * angleDifference(rightLowerArm, rightUpperArm));
        }
    }
}

bool TroyPose::buildUpperArmVectors(
    QSharedPointer<UserSkeleton> skeleton,
    UserSkeleton::UserJoint shoulderJoint,
    UserSkeleton::UserJoint elbowJoint,
    UserSkeleton::UserJoint handJoint,
    const QVector3D &backbone,
    const QVector3D &out,
    const QVector3D &forward,
    const QVector3D &snapCheck,
    QVector3D &shoulder,
    QVector3D &upperArm,
    QVector3D &shoulderTwistVector,
    bool allowSnapping,
    float shoulderForwardHint) {

    // Upper arm vectors
    // The Shoulder is adjusted to more closely simulate Troy's range of motion with the human body.
    // Specifically, Troy's body is roughly double the length of his arms, whereas a humans torso is roughly as wide as their arms are long.
    // This way, when the users hands are close together, Troys should be closer together too.
    float torsoWidth = qAbs((skeleton->userPosition(UserSkeleton::LeftShoulder) - skeleton->userPosition(UserSkeleton::RightShoulder)).length());
    float extensionMultiplier = qBound(0.0f,
                                       torsoWidth*0.45f - (float)skeleton->userPosition(handJoint).distanceToPlane(skeleton->userPosition(UserSkeleton::Neck), out),
                                       torsoWidth/2.0f);
    shoulder = QVector3D(skeleton->userPosition(shoulderJoint) + (extensionMultiplier > torsoWidth/10.0f ? extensionMultiplier : 0.0f) * out);
    upperArm = QVector3D(skeleton->userPosition(elbowJoint) - shoulder);
    upperArm.normalize();
    shoulderTwistVector = QVector3D(project(upperArm, backbone, forward));
    shoulderTwistVector.normalize();

    // Snap check
    bool snap = false;
    if (allowSnapping) {
        // This buffer angle is within +/- 5 degrees of the snapping region. This way, when we are always passed a hint
        // about where the arm was last time, the arm should refrain from jumping from one side to the other really quickly
        // and repeatedly.
        if (angleDifference(snapCheck, shoulderTwistVector) <= 85.0f) {
            snap = true;
            //if (angleDifference(leftShoulderTwistVector, QVector3D::crossProduct(snapCheck, rightOut)) < TroyState::pi/2) {
            // This is less accurate than the line above, but forces the arm to not jump from positive to negative between individual frames.
            // The magic number 80 here is derived from the total allowed region for Troys arm, -20 to 180 degrees: it is the midpoint
            float angle;
            // If we received a hint as to which way to snap (where we were last time),
            if ((shoulderForwardHint != FLT_MAX && shoulderForwardHint > 80.0f) ||
                    finalizeJointAngle(shoulderTwistVector, forward, backbone) > 80.0f) {
                angle = -90.0f;
            } else {
                angle = 110.0f;
            }
            shoulderTwistVector = qSin(angle*(M_PI/180.0f)) * backbone + qCos(angle*(M_PI/180.0f)) * forward;
            shoulderTwistVector.normalize();

            upperArm = project(upperArm, shoulderTwistVector, out);
            upperArm.normalize();
        }
    }
    return snap;
}

void TroyPose::buildLowerArmVectors(
    QSharedPointer<UserSkeleton> skeleton,
    UserSkeleton::UserJoint elbowJoint,
    UserSkeleton::UserJoint handJoint,
    const QVector3D &out,
    const QVector3D &upperArm,
    const QVector3D &shoulderTwistVector,
    //float shoulderOut,
    QVector3D &lowerArm,
    QVector3D &lowerSignVector,
    QVector3D &lowerStart,
    QVector3D &lowerTwistVector) {

    // Left lower arm vectors
    lowerArm = QVector3D(skeleton->userPosition(handJoint) - skeleton->userPosition(elbowJoint));
    lowerArm.normalize();
    lowerSignVector = QVector3D::normal(shoulderTwistVector, upperArm);
    // this factors in for the inversion of the SignVector when the arm is angled in front of the chest
    if (angleDifference(upperArm, out) >= 90.0f) {
        lowerSignVector = -lowerSignVector;
    }
    lowerStart = QVector3D::normal(upperArm, lowerSignVector);
    lowerTwistVector = project(lowerArm, lowerStart, lowerSignVector);
    lowerTwistVector.normalize();
}

float TroyPose::finalizeJointAngle(const QVector3D &vector, const QVector3D &signComparator, const QVector3D &angleComparator) {
    return (angleDifference(vector, signComparator) < 90.0f ? 1 : -1) * angleDifference(vector, angleComparator);
}




void TroyPose::clearCache(int channel) {
    switch (channel) {
    case LeftShoulderForward:
    case LeftShoulderOut:
    case LeftTwist:
    case LeftElbow:
        positions.remove(LeftElbowPoint);
        positions.remove(LeftHandPoint);
    break;
    case RightShoulderForward:
    case RightShoulderOut:
    case RightTwist:
    case RightElbow:
        positions.remove(RightElbowPoint);
        positions.remove(RightHandPoint);
    break;
    }
}

bool TroyPose::canMakePosition(TroyPoint joint) const {
    switch (joint) {
    case LeftShoulderPoint:
    case LeftElbowPoint:
    case LeftHandPoint:
        return hasLeftArm();
    break;
    case RightShoulderPoint:
    case RightElbowPoint:
    case RightHandPoint:
        return hasRightArm();
    break;
    case BodyPoint:
    case NeckPoint:
        return true;
    break;
    default:
        return false;
    break;
    }
}

QVector3D TroyPose::makePosition(TroyPoint joint) const {
    QVector3D ret;
    QMatrix4x4 mapper = QMatrix4x4();
    QQuaternion quat = QQuaternion();

    switch (joint) {
    case LeftShoulderPoint:
        ret = QVector3D(5.5, -1.0, 0);
    break;
    case LeftElbowPoint:
        mapper.translate(0, jointRadius);
        quat *= QQuaternion::fromAxisAndAngle(0.0, 1.0, 0.0, jointAngle(LeftTwist));
        quat *= QQuaternion::fromAxisAndAngle(0.0, 0.0, 1.0, jointAngle(LeftShoulderOut));
        quat *= QQuaternion::fromAxisAndAngle(-1.0, 0.0, 0.0, jointAngle(LeftShoulderForward));
        mapper.rotate(quat);
        mapper.translate(0, jointRadius);

        mapper.translate(0, upperArmLength);
        ret = mapper.map(jointPosition(LeftShoulderPoint));
    break;
    case LeftHandPoint:
        mapper.translate(0, jointRadius);
        quat = QQuaternion::fromAxisAndAngle(0.0, 0.0, 1.0, jointAngle(LeftElbow));
        mapper.rotate(quat);
        mapper.translate(0, jointRadius);

        mapper.translate(0, lowerArmLength);
        ret = mapper.map(jointPosition(LeftElbowPoint));
    break;
    case RightShoulderPoint:
        ret = QVector3D(-5.5, -1.0, 0);
    break;
    case RightElbowPoint:
        mapper.translate(0, jointRadius);
        quat *= QQuaternion::fromAxisAndAngle(0.0, -1.0, 0.0, jointAngle(RightTwist));
        quat *= QQuaternion::fromAxisAndAngle(0.0, 0.0, -1.0, jointAngle(RightShoulderOut));
        quat *= QQuaternion::fromAxisAndAngle(-1.0, 0.0, 0.0, jointAngle(RightShoulderForward));
        mapper.rotate(quat);
        mapper.translate(0, jointRadius);

        mapper.translate(0, upperArmLength);
        ret = mapper.map(jointPosition(RightShoulderPoint));
    break;
    case RightHandPoint:
        mapper.translate(0, jointRadius);
        quat = QQuaternion::fromAxisAndAngle(0.0, 0.0, -1.0, jointAngle(RightElbow));
        mapper.rotate(quat);
        mapper.translate(0, jointRadius);

        mapper.translate(0, lowerArmLength);
        ret = mapper.map(jointPosition(RightElbowPoint));
    break;
    }
    // Cache the joint position
    if (!ret.isNull()) {
        positions.insert(joint, ret);
    }
    return ret;
}

bool TroyPose::hasLeftArm() const {
    return hasJointAngle(LeftShoulderForward) &&
            hasJointAngle(LeftShoulderOut) &&
            hasJointAngle(LeftTwist) &&
            hasJointAngle(LeftElbow);
}

bool TroyPose::hasRightArm() const {
    return hasJointAngle(RightShoulderForward) &&
            hasJointAngle(RightShoulderOut) &&
            hasJointAngle(RightTwist) &&
            hasJointAngle(RightElbow);
}



QVector3D TroyPose::project(const QVector3D &vector, const QVector3D &axis) {
    return QVector3D::dotProduct(vector, axis) * axis;
}

QVector3D TroyPose::project(const QVector3D &vector, const QVector3D &axis1, const QVector3D &axis2) {
    return project(vector, axis1) + project(vector, axis2);
}

qreal TroyPose::angleDifference(const QVector3D &vector1, const QVector3D &vector2) {
    qreal mag1 = vector1.length();
    qreal mag2 = vector2.length();
    if (mag1 != 0 && mag2 != 0) {
        qreal tmp = QVector3D::dotProduct(vector1, vector2) / (mag1 * mag2);
        tmp = qBound(-1.0, tmp, 1.0);
        return qAcos(tmp)*(180.0f/M_PI);
    } else {
        return 0.0;
    }
}
